拖动示教安全平面与安全立方体功能介绍

[TOC]

1 功能概述

在拖动示教（Hand Guiding）过程中，可将环境中的安全平面或安全立方体注册为 world 连杆上的碰撞几何体。力控拖动在周期计算关节指令时，会依据末端（TCP）与这些几何体的相对位置及拖动方向，对运动施加约束：在接近边界时增大阻力或限制继续向越界方向移动，在远离边界时保持正常拖动手感。

本功能与轨迹规划 SpeedL 中的同名几何体共用同一套定义方式（utlDefinePlane、mdlSetLinkCollisionGeometryModel 等）

适用控制形态（推荐配置）

• 控制架构：笛卡尔导纳拖动（CARTESIAN_ADMITTANCE）
• 几何约束作用对象：末端点（TCP）；肘部等中间连杆目前不作为本功能的约束对象

前置条件

• 机器人模型、负载、摩擦等参数已按拖动功能要求标定完成，参见《4.5.机器人力控混合拖动功能包调用介绍文档》。
• 在调用 FPHandGuidingEnable 之前完成安全几何体的建模与挂载。

2 四种典型情形

下列四种情形对应拖动中「安全平面 / 安全立方体」与「起点位于正常区 / 受限区」的组合。理解的关键是：正常区表示起始位姿处于允许拖动的区域；受限区表示起始位姿已处于不允许的一侧，拖动一开始就受到强约束。

• 安全平面：point_direction 所指的内侧半空间为受限区（与平面法向及内侧定义一致）。
• 安全立方体：立方体盒内为受限区（禁入区 / 障碍物），盒外为正常区。

2.1 安全平面 — 起点在正常区

通过三点与方向点定义竖直平面（示例：平面位于 x = 0.6 m），point_direction 指向平面内侧（+X 半空间为内侧）。

• 几何示例：points = {(0.6,0,0), (0.6,0.6,0), (0.6,0.6,0.6)}，point_direction = {0.8, 0, 0}
• 起点：TCP 位于平面外侧（允许侧，例如 x < 0.6）
• 拖动行为：沿 +X 拖动接近平面时，阻力增大并在平面附近难以继续穿入内侧；沿 −X 远离平面时手感正常

flowchart LR
    subgraph 基坐标系俯视图_X-Y
        N["正常区<br/>x 较小一侧<br/>（允许拖动）"]
        P["安全平面<br/>x = 0.6"]
        R["受限区<br/>x 较大一侧<br/>（内侧）"]
    end
    N -->|"沿 +X 拖动"| P
    P -->|"穿入内侧"| R
    N -.->|"沿 −X 拖动<br/>不受限"| N

2.2 安全平面 — 起点在受限区

平面仍由三点 + point_direction 定义，但平面位置更靠近机器人（示例：x = 0.3 m），内侧仍为 +X 方向。

• 几何示例：points = {(0.3,0,0), (0.3,0.3,0), (0.3,0.3,0.3)}，point_direction = {0.8, 0, 0}
• 起点：TCP 已位于平面内侧（受限半空间，例如 x > 0.3）
• 拖动行为：启拖即处于越界侧，向更深处拖动受限；若向平面外侧（−X）拖动，可逐步回到允许区域后恢复与 2.1 类似的手感

flowchart LR
    subgraph 基坐标系俯视图_X-Y
        N2["正常区<br/>x 较小一侧"]
        P2["安全平面<br/>x = 0.3"]
        R2["受限区<br/>起点在此<br/>x 较大一侧"]
    end
    R2 -->|"沿 −X 可回到正常区"| P2
    P2 --> N2
    R2 -.->|"沿 +X 更深处<br/>持续受限"| R2

2.3 安全立方体 — 起点在正常区

以轴对齐长方体描述环境中的禁入区（盒内不可停留或不宜拖入）。六个盒面等价于六个安全平面：从盒外朝盒内拖动、接近盒面时触发约束。

• 几何示例：origin = {0.5, 0, 0, 0, 0, 0}，size = {0.2, 0.2, 0.2}（中心在 (0.5,0,0)，半边长 0.1 m）
• 起点：TCP 位于盒外部（正常区）
• 拖动行为：盒外可正常拖动；朝盒心方向拖动时，接近盒面阻力增大，难以穿入盒内；远离盒体时不受限

flowchart LR
    subgraph 基坐标系俯视图_X-Y
        N3["正常区<br/>盒外<br/>● 起点"]
        F3["盒面<br/>x ≈ 0.4 / 0.6"]
        R3["受限区<br/>盒内"]
    end
    N3 -->|"朝盒内拖动"| F3
    F3 -->|"穿入盒内"| R3
    N3 -.->|"盒外拖动<br/>不受限"| N3

2.4 安全立方体 — 起点在受限区

同样将立方体作为禁入区，但盒体更大，使起点落在盒内。

• 几何示例：origin = {0, 0, 0, 0, 0, 0}，size = {1.1, 1.1, 1.1}
• 起点：TCP 位于盒内部（受限区）
• 拖动行为：启拖即处于禁入区，盒内拖动持续受限；朝盒外方向拖动，在盒面附近受阻，穿出盒外后恢复与 2.3 类似的手感

flowchart LR
    subgraph 基坐标系俯视图_X-Y
        N4["正常区<br/>盒外"]
        F4["盒面<br/>边长 1.1 m"]
        R4["受限区<br/>● 起点在盒内"]
    end
    R4 -->|"朝盒外拖动"| F4
    F4 --> N4
    R4 -.->|"盒内更深处<br/>持续受限"| R4

2.5 情形对照表

说明：拖动示教中的安全立方体按环境障碍物语义使用（盒内受限、盒外正常）

3 几何体配置说明

3.1 几何类型编码

挂载时统一使用：

link_name = "world"

表示几何体固定在环境 / 基坐标系下，不随连杆运动。

3.2 安全平面

平面方程：Ax + By + Cz + D = 0。若点 P 位于内侧，则 Ax + By + Cz + D > 0。

• 使用 utlDefinePlane(points, point_direction, plane) 由三点确定平面，并由 point_direction 上的参考点确定内侧朝向。
• 前三点不得重合、不得共线；point_direction 不得落在平面上。
• 拖动示例中为无限平面（未设置 extent）；若需有限矩形平面，可设置 plane.setExtent({ex, ey}) 并配合 origin 标定平面局部坐标系，规则见《3.9.安全平面功能介绍》。

3.3 安全立方体（禁入区）

• Box::size_ 为沿基坐标轴的边长 [sx, sy, sz]。
• origin 为盒中心相对基坐标系的位姿；若 origin 为零位姿，则盒中心在基坐标原点。
• 拖动场景下，立方体表示禁止进入的区域：TCP 位于盒内时为受限区；在盒外为正常区。朝盒内方向拖动且接近盒面时触发约束；已在盒内时启拖即受限。
• 盒体不宜过大，否则易使常态工作位姿落在盒内（对应 2.4 受限启拖）；禁入区宜按现场障碍或禁区实际尺寸布置。

3.4 与拖动方向的关系

拖动使能向量 direction 为 [dx, dy, dz, drx, dry, drz]（0 关闭，1 开启）。安全几何体按 TCP 位置判断，再与当前开启的平移/旋转方向叠加：

• 仅开放 x/y/z 时，旋转拖动不受几何体额外约束

4 接口与调用流程

整体流程：建模与挂载安全几何体 → 使能拖动 → 周期更新拖动输出。

sequenceDiagram
    participant App as 应用层
    participant Model as 模型接口
    participant Pack as 拖动功能包

    App->>Model: utlDefinePlane / 构造 Box
    App->>Model: mdlSetLinkCollisionGeometryModel("world", ...)
    App->>Pack: FPHandGuidingEnable(enable)
    loop 控制周期
        App->>Pack: FPHandGuidingUpdate(update, out)
        App->>App: 下发 JointCommand
    end

4.1 定义并挂载几何体

#include "aral/robot_library_interface.hpp"
#include "aral/shapes.hpp"

using namespace ARAL;

// ---------- 示例 A：安全平面（无限平面）----------
std::vector<interface::Array3d> points = {
    {0.6, 0.0, 0.0}, {0.6, 0.6, 0.0}, {0.6, 0.6, 0.6}
};
interface::Array3d point_direction = {0.8, 0.0, 0.0};

geometric_shapes::Plane plane(0, 0, 0, 0);
int ret = robot->utlDefinePlane(points, point_direction, plane);
if (ret < 0) { /* 平面定义失败 */ }

geometric_shapes::ShapePtr shape =
    std::make_shared<geometric_shapes::Plane>(plane);
interface::RLPose origin = {0, 0, 0, 0, 0, 0};
ret = robot->mdlSetLinkCollisionGeometryModel("world", {shape}, {origin});

// ---------- 示例 B：安全立方体（禁入区，盒内受限）----------
geometric_shapes::ShapePtr box =
    std::make_shared<geometric_shapes::Box>(0.2, 0.2, 0.2);
interface::RLPose box_origin = {0.5, 0.0, 0.0, 0.0, 0.0, 0.0};
ret = robot->mdlSetLinkCollisionGeometryModel("world", {box}, {box_origin});

相关接口说明见《3.9.安全平面功能介绍》第 2 节。

4.2 使能与周期更新

#include "aral/robot_feat_pack.hpp"  // 或 robot_featpack.hpp（Scene 版本）

pack::HandGudingEnable enable;
enable.q   = q_init;
enable.qd  = qd_init;
enable.qdd = qdd_init;
enable.t_w = tool_workpiece;
enable.direction = {1, 1, 1, 0, 0, 0};  // 仅平移拖动
enable.command   = pack::CommandType::POSITION;
enable.singular  = pack::SingularProtectType::STOP;
enable.sensor    = interface::FTSensorType::END;
// 设置 jnt_damp、end_damp、force_threshold、force_limit ...

ret = pack::FPHandGuidingEnable(robot, enable);
if (ret < 0) { /* 使能失败 */ }

pack::HandGudingUpdate update;
update.state.q      = actual_q;
update.state.qd     = actual_qd;
update.state.qdd    = actual_qdd;
update.state.torque = actual_torque;
update.state.sensor = sensor_wrench;
update.task_frame   = {0, 0, 0, 0, 0, 0};  // BASE

interface::JointCommand cmd;
ret = pack::FPHandGuidingUpdate(robot, update, cmd);

说明

• HandGudingEnable、HandGudingUpdate 字段含义见《4.5.机器人力控混合拖动功能包调用介绍文档》。
• 使用 Scene 接口时，对应 pack::FPHandGuidingEnable(log, state, controller, enable, ctx) 与 FPHandGuidingUpdate(..., ctx)，需在多次 Update 间保持同一 HandGuidingContext。
• 非六轴全开的拖动使能下，功能包内部固定走笛卡尔导纳分支，并与安全几何体约束协同工作。

4.3 卸载或更换几何体

更换安全区域前，可先卸载 world 上已有几何体，再重新设置，避免多个重叠约束：

robot->mdlUnsetLinkCollisionGeometryModel("world", {});  // 按实际 API 卸载全部或指定名称

5 注意事项

1. 定义顺序：必须先 mdlSetLinkCollisionGeometryModel，再 FPHandGuidingEnable；运行中修改几何时建议先停止拖动。
2. 坐标系一致：平面三点、point_direction、立方体 origin 均在基坐标系下描述；BASE 拖动时 task_frame 为零位姿。
3. 内侧方向：平面 point_direction 标错会导致「允许 / 禁止」半空间颠倒，启拖前应在仿真或示教器中确认 TCP 位于预期侧。
4. 禁入盒尺寸与位置：盒体应包住现场禁区；盒过大易使常态位姿落在盒内（2.4 受限启拖）。启拖前确认 TCP 在盒外或盒内是否符合预期。
5. 仅约束 TCP：肘部、腕部等中间点可能进入几何体外部，但本功能不以之为判据；布置几何时以 TCP 轨迹为准。

6 相关文档